#!/usr/bin/env python

import roslib
import rospy
import math
import tf
import geometry_msgs.msg
import time
import numpy as np
import csv
from std_msgs.msg import Float64MultiArray
from scipy.spatial.transform import Rotation as R
from sensor_msgs.msg import LaserScan
from gazebo_msgs.msg import ModelState


if __name__ == '__main__':
	# init the node
	rospy.init_node('visual_inspection', anonymous=True)

	# read the tf info form the laserscan to the world
	listener = tf.TransformListener()

	# control the lasersky 2 revolute joints
	lasersky1 = rospy.Publisher('laser_sky1/command', Float64MultiArray, queue_size=10)
	lasersky2 = rospy.Publisher('laser_sky2/command', Float64MultiArray, queue_size=10)
	pogo1     = rospy.Publisher('pogo_group1/command', Float64MultiArray, queue_size=10)
	pogo2     = rospy.Publisher('pogo_group2/command', Float64MultiArray, queue_size=10)
	pogo3     = rospy.Publisher('pogo_group3/command', Float64MultiArray, queue_size=10)
	jichuag   = rospy.Publisher('jichuag/command', Float64MultiArray, queue_size=10)
	houduan   = rospy.Publisher('gazebo/set_model_state',ModelState, queue_size=10)
	time.sleep(0.5)

	# home point
	lasersky1.publish(Float64MultiArray(data=[0, 0]))
	lasersky2.publish(Float64MultiArray(data=[0, 0]))

	pogo1.publish(Float64MultiArray(data=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
	pogo2.publish(Float64MultiArray(data=[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
	pogo3.publish(Float64MultiArray(data=[0, 0, -0.0, 0, 0, -0.0, 0, 0, -0.0, 0, 0, -0.0]))
	
	jichuag.publish(Float64MultiArray(data=[0,0,0,0,0]))
	time.sleep(1)

	# # No.1 laser phase
	input("Input 1 to start laser phase!")
	laser1_joint=[0.0,0.0]
	laser2_joint=[0.0,0.0]
	lasersky1.publish(Float64MultiArray(data=laser1_joint))
	lasersky2.publish(Float64MultiArray(data=laser2_joint))

	print("laser scan started")
	# must sleep 1 second
	time.sleep(1)
	(trans1,rot1) = listener.lookupTransform('/laser_sky1_link2', '/world', rospy.Time(0))
	(trans2,rot2) = listener.lookupTransform('/laser_sky2_link2', '/world', rospy.Time(0))

	# target points' position array in world 

	target1_p_all_x = [0.7, 0.7, 0.7]   #for sky_laser1
	target1_p_all_y = [0.1, 0.0, 0.2]
	target1_p_all_z = [1.9, 2.1, 2.3]
	target2_p_all_x = [1.0, 1.0, 1.0]   #for sky_laser2
	target2_p_all_y = [0.62,0.61,0.6]
	target2_p_all_z = [1.9, 2.1 ,2.3]
	for i in range(len(target1_p_all_x)):
		target1_p_w = np.array([[target1_p_all_x[i]],[target1_p_all_y[i]],[target1_p_all_z[i]]])
		R1 = R.from_quat(rot1)
		R1 = R1.as_dcm()
		target1_p_h1 = np.matmul(R1, target1_p_w)
		x1 = trans1[0] + target1_p_h1[0][0]
		y1 = trans1[1] + target1_p_h1[1][0]
		z1 = trans1[2] + target1_p_h1[2][0]
		z1=-z1
		dist1 = math.sqrt(x1*x1 + y1*y1 + z1*z1)
		lasersky1_angle1 = math.atan(y1/x1)
		lasersky1_angle2 = math.atan(z1/math.sqrt(x1*x1 + y1*y1))
		laser1_joint[0]=lasersky1_angle1
		laser1_joint[1]=lasersky1_angle2
		#print(x1, y1, z1, laser1_joint)
		if (abs(lasersky1_angle1) >= 0.001 or abs(lasersky1_angle2) >= 0.001) and x1 >= 0:
			lasersky1.publish(Float64MultiArray(data=laser1_joint))
		elif (abs(lasersky1_angle1) >= 0.001 or abs(lasersky1_angle2) >= 0.001) and x1 < 0:
			laser1_joint[0]=laser1_joint[0]+math.pi
			lasersky1.publish(Float64MultiArray(data=laser1_joint))	
		time.sleep(6)

	for i in range(len(target2_p_all_x)):
		target2_p_w = np.array([[target2_p_all_x[i]],[target2_p_all_y[i]],[target2_p_all_z[i]]])
		R2 = R.from_quat(rot2)
		R2 = R2.as_dcm()
		target2_p_h2 = np.matmul(R2, target2_p_w)
		x2 = trans2[0] + target2_p_h2[0][0]
		y2 = trans2[1] + target2_p_h2[1][0]
		z2 = trans2[2] + target2_p_h2[2][0]
		z2=-z2
		dist2 = math.sqrt(x2*x2 + y2*y2 + z2*z2)
		lasersky2_angle1 = math.atan(y2/x2)
		lasersky2_angle2 = math.atan(z2/math.sqrt(x2*x2 + y2*y2))
		laser2_joint[0]=lasersky2_angle1
		laser2_joint[1]=lasersky2_angle2
		#print(x2, y2, z2, laser2_joint)
		if (abs(lasersky2_angle1) >= 0.001 or abs(lasersky2_angle2) >= 0.001) and x1 >= 0:
			lasersky2.publish(Float64MultiArray(data=laser2_joint))
		elif (abs(lasersky2_angle1) >= 0.001 or abs(lasersky2_angle2) >= 0.001) and x1 < 0:
			laser1_joint[0]=laser1_joint[0]+math.pi
			lasersky2.publish(Float64MultiArray(data=[laser2_joint]))	
		time.sleep(6)

	# # No.2 pogo phase, firstly pogo_group3 go up
	input("Input 2 to start pogoup phase!")
	# Because houduan and agv have some collision during laser phase
	# so we have to move houduan to its initial pose
	houduanstate=ModelState()
	houduanstate.pose.position.x=0.6
	houduanstate.pose.position.y=0.0
	houduanstate.pose.position.z=1.77
	houduanstate.pose.orientation.x=0.0
	houduanstate.pose.orientation.y=0.0
	houduanstate.pose.orientation.z=0.0
	houduanstate.model_name="houduanbot"
	houduanstate.reference_frame="world"
	houduan.publish(houduanstate)
	time.sleep(10)
	startz=0.0
	targetz=0.4
	step=3400
	for i in range(step):
		zi=startz+(targetz-startz)/step*i
		pogo3.publish(Float64MultiArray(data=[0, 0, zi, 0, 0, zi, 0, 0, zi, 0, 0, zi]))
		time.sleep(0.025)
		print(zi)
	time.sleep(5)
	# # second phase pogo_group3 go ahead
	startx=0.0
	targetx=-0.5
	step=2400   
	for i in range(step):
		xi=startz+(targetx-startx)/step*i
		pogo3.publish(Float64MultiArray(data=[xi, 0, 0.4, -xi, 0, 0.4, -xi, 0, 0.4, xi, 0, 0.4]))
		time.sleep(0.08)  
		print(xi)

	# # No.3 jichuag phase
	input("Input 3 to start jichuag phase!")
	# first, machining the back side of zhongduan
	jichuag.publish(Float64MultiArray(data=[2,-0.4,0,0,0]))
	time.sleep(15)
	jichuag.publish(Float64MultiArray(data=[2,-0.4,0.32,0,0]))
	time.sleep(3)
	jichuag.publish(Float64MultiArray(data=[2,0.4,0.32,0,0]))
	time.sleep(6)
	jichuag.publish(Float64MultiArray(data=[2,0.4,0,0,0]))
	time.sleep(3)

	# second, machining the left side of zhognduan
	jichuag.publish(Float64MultiArray(data=[3.23,1.53,0,0,-1.5708]))
	time.sleep(15)
	jichuag.publish(Float64MultiArray(data=[3.23,1.53,0.64,0,-1.5708]))
	time.sleep(7)
	jichuag.publish(Float64MultiArray(data=[3.23,1.53,1.07,0,-1.5708]))
	time.sleep(5)
	jichuag.publish(Float64MultiArray(data=[3.72,1.53,1.07,0,-1.5708]))
	time.sleep(5)
	jichuag.publish(Float64MultiArray(data=[3.72,1.53,0.64,0,-1.5708]))
	time.sleep(5)
	jichuag.publish(Float64MultiArray(data=[3.72,1.53,0,0,0]))
	time.sleep(5)

	# third, machining the top side of zhognduan
	jichuag.publish(Float64MultiArray(data=[4.4,0.2,0.24,0,0]))
	time.sleep(13)
	jichuag.publish(Float64MultiArray(data=[4.4,-0.2,0.24,0,0]))
	time.sleep(5)
	jichuag.publish(Float64MultiArray(data=[5.0,-0.2,0.24,0,0]))
	time.sleep(5)
	jichuag.publish(Float64MultiArray(data=[5.0,0.2,0.24,0,0]))
	time.sleep(5)
	jichuag.publish(Float64MultiArray(data=[0,0,0,0,0]))
	time.sleep(5)